#include <stdio.h>
#include <stdlib.h>
#include <time.h>

#include "system.h"
#include "altera_avalon_pio_regs.h"
#include "alt_types.h"

#include "globals.h"
#include "recording.h"

/* Record Queue - FIFO */
static struct record_t * record_list = NULL;
static struct record_t * record_tail = NULL;
static struct record_t * current_record = NULL;

/* Record Building */
static struct record_t * new_record = NULL;
static clock_t new_timestamp;

/* Recording flag */
static char recording_mode = 0;

struct record_t * GetFirstRecord(void)
{
    return record_list;
}

struct record_t * GetNextRecord(void)
{
    if (current_record == NULL)
    {
        current_record = record_list;
        return current_record;
    }

    
    return current_record->next_record;
}

void AddRecordToQueue(const struct record_t * record)
{
    new_record = (struct record_t *)record;
    
    new_record->next_record = NULL;
    
    if (record_list == NULL)
    {
        record_list = new_record;
        record_tail = record_list;
    }
    else
    {
        record_tail->next_record = new_record;
        record_tail = record_tail->next_record;
    }
    
    return;
}

void DeleteRecordFromQueue(const struct record_t * record)
{
    struct record_t * a_record = record_list;
    
    if (record == NULL)
    {
        return;
    }
    
    if (a_record == record)
    {
        record_list = record_list->next_record;
        free(a_record);
    }
    else
    {
        while (a_record != NULL)
        {
            if (a_record->next_record == record)
            {
                a_record->next_record = record->next_record;
                free((struct record_t *)record);
                break;
            }
            
            a_record = a_record->next_record;
        }
    }
    
    return;
}

void StartRecord(const unsigned char event_code, const unsigned long value)
{
    if (recording_mode)
    {
        if (new_record != NULL)
        {
            free(new_record);
        }
        
        new_record = (struct record_t *)malloc(sizeof(struct record_t));
        if (new_record == NULL)
        {
            return;
        }
        
        new_record->event_code = event_code;
        new_timestamp = clock();
    }
    
    return;
}

void EndRecord(void)
{
    if (recording_mode)
    {
        clock_t current_timestamp = clock();
        clock_t time_difference = current_timestamp - new_timestamp;
        
        if (new_record == NULL)
        {
            return;
        }
        
        /* TODO: Wrap around may break this - check later */
        new_record->clocks = time_difference;
        
        AddRecordToQueue(new_record);
        
        new_record = NULL;
    }
    
    return;
}

void EnableRecording(void)
{
    recording_mode = 1;
    
    ClearRecordList();
    
    return;
}

void DisableRecording(void)
{
    recording_mode = 0;
    return;
}

char RecordingMode(void)
{
    return recording_mode;
}

void RecordWait(clock_t wait)
{
    clock_t start = clock();
    clock_t now = clock();
    
    while ((now - start) < wait)
    {
        now = clock();
    }
    
    return;
}

void RecordEvent(unsigned int event, unsigned long int value)
{
    struct record_t * record = NULL;
    
    record = (struct record_t *)malloc(sizeof(struct record_t));
    if (record)
    {
        record->event_code = event;
        
        AddRecordToQueue(record);
    }
    
    printf("Record %d added\n", event);
    
    return;
}

op_fnptr GetOpFromEvtCode(unsigned int event)
{
    switch (event)
    {
        case EVENT_STOP:
            return CoreWait;
            break;
        
        case EVENT_ROTATE_LEFT:
            return MoveBaseLeft;
            break;
        
        case EVENT_ROTATE_RIGHT:
            return MoveBaseRight;
            break;
        
        case EVENT_SHOULDER_UP:
            return MoveShoulderUp;
            break;
        
        case EVENT_SHOULDER_DOWN:
            return MoveShoulderDown;
            break;
            
        case EVENT_ELBOW_UP:
            return MoveElbowUp;
            break;
            
        case EVENT_ELBOW_DOWN:
            return MoveElbowDown;
            break;
        
        case EVENT_GRIPPER_INVERT:
            return InvertGripper;
            break;
        
        case EVENT_HOME:
            return Home;
            break;
        
        case EVENT_TEST:
            return Test;
            break;
    }
    
    return NULL;
}

void ClearRecordList(void)
{
    struct record_t * record = NULL;
    struct record_t * next_rec = NULL;
    
    record = record_list;
    while (record != NULL)
    {
        next_rec = record->next_record;
        free(record);
        record = next_rec;
    }
    
    return;
}
